﻿package systems.traer 
{
	import flash.geom.Vector3D;
	
	public class RungeKuttaIntegrator implements Integrator
	{
		
		private var originalPositions:Vector.<Vector3D>;
		private var originalVelocities:Vector.<Vector3D>;
		
		private var k1Forces:Vector.<Vector3D>;
		private var k1Velocities:Vector.<Vector3D>;
		
		private var k2Forces:Vector.<Vector3D>;
		private var k2Velocities:Vector.<Vector3D>;
		
		private var k3Forces:Vector.<Vector3D>;
		private var k3Velocities:Vector.<Vector3D>;
		
		private var k4Forces:Vector.<Vector3D>;
		private var k4Velocities:Vector.<Vector3D>;
		
		private var s:ParticleSystem;
		
		
		public function RungeKuttaIntegrator(_s:ParticleSystem) 
		{
			s = _s;
			originalPositions = new Vector.<Vector3D>();
			originalVelocities = new Vector.<Vector3D>();
			
			k1Forces = new Vector.<Vector3D>();
			k1Velocities = new Vector.<Vector3D>();
			
			k2Forces = new Vector.<Vector3D>();
			k2Velocities = new Vector.<Vector3D>();
			
			k3Forces = new Vector.<Vector3D>();
			k3Velocities = new Vector.<Vector3D>();
			
			k4Forces = new Vector.<Vector3D>();
			k4Velocities = new Vector.<Vector3D>();
			
		}
		
		private final function allocateParticles():void 
		{
			
			while ( s.particles.length > originalPositions.length )
			{
				
				originalPositions.push( new Vector3D() );
				originalVelocities.push( new Vector3D() );
				
				k1Forces.push( new Vector3D() );
				k1Velocities.push( new Vector3D() );
				
				k2Forces.push( new Vector3D() );
				k2Velocities.push( new Vector3D() );
				
				k3Forces.push( new Vector3D() );
				k3Velocities.push( new Vector3D() );
				
				k4Forces.push( new Vector3D() );
				k4Velocities.push( new Vector3D() );
				
			}
		}
		
		public final function step(deltaT:Number):void
		{
			
			allocateParticles();
			
			
			var originalPosition:Vector3D;
			var originalVelocity:Vector3D;
			
			var k1Velocity:Vector3D, k2Velocity:Vector3D, k3Velocity:Vector3D, k4Velocity:Vector3D;
			var k1Force:Vector3D, k2Force:Vector3D, k3Force:Vector3D, k4Force:Vector3D;
			
			var halfDelta:Number = deltaT * .5;
			var delta6:Number = deltaT / 6;
			
			
			var i:int;
			var len:int = s.particles.length;
			var p:Particle;
			
			for (i = 0; i < len; i++)
			{
				p = s.particles[i];
				
				if ( !p.fixed )
				{
					originalPositions[i] = p.position.clone();
					originalVelocities[i] = p.velocity.clone();
				}
				
				p.clearForce();
				
			}
			s.applyForces();
			
			for ( i = 0; i < len; ++i )
			{
				p = s.particles[i];
				if ( !p.fixed )
				{
					k1Forces[i] = p.force.clone( );
					k1Velocities[i] = p.force.clone( );
				}
				
				p.clearForce();
			}
			
			
			for ( i = 0; i < len; ++i )
			{
				p = s.particles[i];
				if ( !p.fixed )
				{
					originalPosition = originalPositions[i];
					k1Velocity = k1Velocities[i];
					
					p.position.x = originalPosition.x + k1Velocity.x * halfDelta;
					p.position.y = originalPosition.y + k1Velocity.y * halfDelta;
					p.position.z = originalPosition.z + k1Velocity.z * halfDelta;
					
					originalVelocity = originalVelocities[i];
					k1Force = k1Forces[i];
					
					p.velocity.x = originalVelocity.x + k1Force.x * halfDelta / p.mass;
					p.velocity.y = originalVelocity.y + k1Force.y * halfDelta / p.mass;
					p.velocity.z = originalVelocity.z + k1Force.z * halfDelta / p.mass;
				}
			}
			
			s.applyForces();
			
			
			for ( i = 0; i < len; ++i )
			{
				p = s.particles[i];
				if ( !p.fixed )
				{
					k2Forces[i] = p.force.clone( );
					k2Velocities[i] = p.velocity.clone( );
				}
				p.clearForce();	// and clear the forces now that we are done with them
			}
			
			for ( i = 0; i < len; ++i )
			{
				p = s.particles[i];
				if ( !p.fixed )
				{
					originalPosition = originalPositions[i];
					k2Velocity = k2Velocities[i];
					
					p.position.x = originalPosition.x + k2Velocity.x * halfDelta;
					p.position.y = originalPosition.y + k2Velocity.y * halfDelta;
					p.position.z = originalPosition.z + k2Velocity.z * halfDelta;
					
					originalVelocity = originalVelocities[i];
					k2Force = k2Forces[i];
					
					p.velocity.x = originalVelocity.x + k2Force.x * halfDelta / p.mass;
					p.velocity.y = originalVelocity.y + k2Force.y * halfDelta / p.mass;
					p.velocity.z = originalVelocity.z + k2Force.z * halfDelta / p.mass;
				}
			}
			s.applyForces();
			
			for ( i = 0; i < len; ++i )
			{
				p = s.particles[i];
				
				if ( !p.fixed )
				{
					k3Forces[i] = p.force.clone();
					k3Velocities[i] = p.velocity.clone();
				}
				
				p.clearForce();	// and clear the forces now that we are done with them
			}
			
			
			
			for ( i = 0; i < len; ++i )
			{
				p = s.particles[i];
				if ( !p.fixed )
				{
					originalPosition = originalPositions[i];
					k3Velocity = k3Velocities[i];
					
					p.position.x = originalPosition.x + k3Velocity.x * deltaT;
					p.position.y = originalPosition.y + k3Velocity.y * deltaT;
					p.position.z = originalPosition.z + k3Velocity.z * deltaT;
					
					originalVelocity = originalVelocities[i];
					k3Force = k3Forces[i];
					
					p.velocity.x = originalVelocity.x + k3Force.x * deltaT / p.mass;
					p.velocity.y = originalVelocity.y + k3Force.y * deltaT / p.mass;
					p.velocity.z = originalVelocity.z + k3Force.z * deltaT / p.mass;

				}
			}
			
			s.applyForces();
			
			
			for ( i = 0; i < len; ++i )
			{
				p = s.particles[i] as Particle;
				if ( !p.fixed )
				{
					k4Forces[i] = p.force.clone();
					k4Velocities[i] = p.velocity.clone();
				}
			}
			
			
			
			for ( i = 0; i < len; ++i )
			{
				p = s.particles[i];
				p.age += deltaT;
				if ( !p.fixed )
				{
					
					// update position
					
					originalPosition = originalPositions[i];
					k1Velocity = k1Velocities[ i ];
					k2Velocity = k2Velocities[ i ];
					k3Velocity = k3Velocities[ i ];
					k4Velocity = k4Velocities[ i ];
					
					p.position.x = originalPosition.x + deltaT / 6 * ( k1Velocity.x + 2.0*k2Velocity.x + 2.0*k3Velocity.x + k4Velocity.x );
					p.position.y = originalPosition.y + deltaT / 6 * ( k1Velocity.y + 2.0*k2Velocity.y + 2.0*k3Velocity.y + k4Velocity.y );
					p.position.z = originalPosition.z + deltaT / 6 * ( k1Velocity.z + 2.0*k2Velocity.z + 2.0*k3Velocity.z + k4Velocity.z );
					
					// update velocity
					
					originalVelocity = originalVelocities[ i ];
					k1Force = k1Forces[ i ];
					k2Force = k2Forces[ i ];
					k3Force = k3Forces[ i ];
					k4Force = k4Forces[ i ];
					
					p.velocity.x = originalVelocity.x + deltaT / ( 6.0 * p.mass ) * ( k1Force.x + 2.0*k2Force.x + 2.0*k3Force.x + k4Force.x );
					p.velocity.y = originalVelocity.y + deltaT / ( 6.0 * p.mass ) * ( k1Force.y + 2.0*k2Force.y + 2.0*k3Force.y + k4Force.y );
					p.velocity.z = originalVelocity.z + deltaT / ( 6.0 * p.mass ) * ( k1Force.z + 2.0*k2Force.z + 2.0*k3Force.z + k4Force.z );
				}
			}
		}
	}
}